/**
    ATClab - scriptable Air Traffic Control simulations.
    Copyright (C) 2010 openatclab@gmail.com

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
**/

#include "airspace.h"

#include "atc.h"
#include "aircraft_agent.h"
#include "aircraft_data.h"
#include "area_definition.h"
#include <algorithm>
#include <q3pointarray.h>
#include <qmatrix.h>

#ifdef WIN32
	#include <assert.h>
#endif


using namespace atc;

using std::deque;
using std::pair;
using std::string;


////////////////////////////////////////////////////////////////////////////////
//
// atc::Airspace
//
//------------------------------------------------------------------------------
// [public] construction
//

/*!
 *
 */
Airspace::Airspace()
	  : _aircraft()
	  , _conflicts()
	  , _sector()	  
	  , _hdoms( NAN )
	  , _vdoms( NAN )
{}

/*!
 *
 */
Airspace::~Airspace() {}


//------------------------------------------------------------------------------
// [public] setters
//

/*!
 * Add an aircraft to the airspace.
 *
 * The aircraft agent's destruction is handled by the QObject parent that was 
 * set at construction time. The parent may or may not be the airspace.
 */
void Airspace::add_aircraft( AircraftAgent *ac ) {
	_aircraft.insert( std::make_pair( ac->callsign(), ac ) );
	connect( 
		    ac,   SIGNAL( event_recorded( KeyValues & ) )
		  , this, SIGNAL( event_recorded( KeyValues & ) )
		);
}

/*!
 *
 */
void Airspace::define_sector( const AreaDefinition &def ) {
	Q3PointArray pts;
	_sector = QRegion( area_points( def, pts ) );
}

/*!
 *
 */
void Airspace::add_non_sector( const AreaDefinition &def ) {
	Q3PointArray pts;
	_nonsector.push_back( QRegion( area_points( def, pts ) ) );
}

//------------------------------------------------------------------------------
// [public] initialization
//

/*!
 *
 */
void Airspace::initialize() {
	std::for_each( 
			_aircraft.begin(), _aircraft.end(), InitialControlState( this ) 
	);
}

/*!
 *
 */
bool Airspace::point_in_sector( const Point &p ) const {
	QPoint pos( p.first, p.second );

	QRegionList::const_iterator non = _nonsector.begin();
	for ( ; non != _nonsector.end(); ++non ) {
		if ( non->contains( pos ) ) {
			return false;
		}
	}

	return _sector.contains( pos );
}


//------------------------------------------------------------------------------
// [public slots]
//

/*!
 * Advance the airspace by time period
 *
 * @param ms time in milli-seconds
 */
void Airspace::advance( int ms ) {

	std::for_each( _aircraft.begin(), _aircraft.end(), AdvancePosition( ms ) );
	std::for_each( 
			_aircraft.begin(), _aircraft.end(), UpdateControlState( this ) 
	);

//	std::for_each( 
//			_aircraft.begin(), _aircraft.end()
//		  , DetectConflicts( &_aircraft, _hdoms, _vdoms ) 
//	);

	//
	// Test for missed acceptance
	//

	ACAgentMap::iterator aait;
	for ( aait = _aircraft.begin(); aait != _aircraft.end(); ++aait ) {
		AircraftAgent* aa = aait->second;
		if ( aa->control() == CS_PROPOSED && point_in_sector(aa->position()) ){
			if ( !aa->has_missed_acceptance() ) {
				aa->flag_missed_acceptance();
				emit sig_missed_acceptance();
			}
		}
	}

	//
	// Conflict Detection
	//

	ConflictMap::iterator cit;

	for ( cit = _conflicts.begin(); cit != _conflicts.end(); ++cit ) cit->second = false;

	ACAgentMap::iterator it0, it1;
	AircraftAgent *ac0, *ac1;

	for ( it0 = _aircraft.begin(); it0 != _aircraft.end(); ) {
		ac0 = it0->second;
		++it0;
		if ( ! ac0->is_active() ) continue;

		for ( it1 = it0; it1 != _aircraft.end(); ++it1 ) {
			ac1 = it1->second;
			if ( ! ac1->is_active() ) continue;
			
			if ( ac0->collides_with( ac1, _hdoms, _vdoms ) ) {
				std::string call0 = ac0->callsign();
				std::string call1 = ac1->callsign();

				if ( call1 < call0 ) {
					call0 = call1;
					call1 = ac0->callsign();
				}

				ConflictPair cp( call0, call1 );
				
				if ( (cit = _conflicts.find( cp )) != _conflicts.end() ) cit->second = true;
				else {
					_conflicts[cp] = true;
					emit sig_conflict_detected( ac0->callsign(), ac1->callsign() );
				}
			}
		}
	}

	//
	ACAgentMap::iterator ait;
	for ( ait = _aircraft.begin(); ait != _aircraft.end(); ++ait ) ait->second->clear_conflict();

	// remove old conflicts
	for ( cit = _conflicts.begin(); cit != _conflicts.end(); ) {
		if ( ! cit->second ) _conflicts.erase( cit++ );
		else {
			ConflictPair a = cit->first;
			_aircraft[ a.first  ]->set_conflict();
			_aircraft[ a.second ]->set_conflict();
			++cit;
		}
	}

	//
	//
	//
	emit advanced();
}


//------------------------------------------------------------------------------
// [private] implementation
//

/*!
 *
 */
Q3PointArray& Airspace::area_points( 
		const AreaDefinition &area_def
	  , Q3PointArray &pts_ref 
) {
	// validate point array
	if ( ! pts_ref.isNull() ) {
		throw airspace_error( "Cannot process area: point array is not empty!" );
	}
	
	// process area's descriptors
	AreaDefinition::const_iterator it = area_def.begin();
	for ( ; it != area_def.end(); ++it) {

		switch ( (*it)->rtti ) {
		
		case ( AreaDescriptor::RTTI_VERTEX ): {
			pts_ref.putPoints( pts_ref.size(), 1, (*it)->x, (*it)->y ); 
			break;
		}
		case ( AreaDescriptor::RTTI_ARC ): {
			ArcDescriptor *arc = static_cast< ArcDescriptor* >( *it );

			QPoint p0( (int)( arc->x ), (int)( arc->y ) );
			int r = (int)( arc->r );

			if ( 0 == pts_ref.size() ) {
				// assume 360 single area arc descriptor
				// @todo: assert( only area descriptor )
				pts_ref.makeEllipse( p0.x() - r, p0.y() - r , 2 * r, 2 * r );
			}
			else {
				++it;  // need next point also to limit arc

				assert( it != area_def.end() );

				QPoint pA( pts_ref.at( pts_ref.size() - 1 ) );
				QPoint pB( (*it)->x, (*it)->y );

				bool minor = true;  // TODO:??

				double bA = bearing(p0, pA);
				double bB = bearing(p0, pB);

				if ( (bB - bA > 180 && minor) || (bB - bA < 180 && !minor) ) {
					double tmp = bA;
					bA = bB;
					bB = tmp;
				}

				double angle = bB - bA; 

				Q3PointArray arcPts;
				arcPts.makeArc(
					p0.x() - r, p0.y() - r, 2 * r, 2 * r, 
					(floor)(bA * 16 + 0.5), (floor)(angle * 16 + 0.5)
				);

				// Note: do not include pA or pB in final pts.
				pts_ref.putPoints( pts_ref.size() - 1, arcPts.size(), arcPts );
			}			
			
			break;
		}
		case ( AreaDescriptor::RTTI_ELLIPSE ): {
			EllipseDescriptor *e 
				  = static_cast< EllipseDescriptor* >( *it );

			QPoint p0( e->x, e->y );
			int w = (int)( e->w );
			int h = (int)( e->h );
			int a =      ( e->a );

			// @todo: assert only one area descriptor (this)
			pts_ref.makeEllipse( p0.x() - w/2, p0.y() - h/2 , w, h );

			if ( a ) {
				QMatrix matrix;
				matrix.rotate( -a );
				pts_ref.translate( -p0.x(), -p0.y() );
				pts_ref = matrix.map( pts_ref );
				pts_ref.translate(  p0.x(),  p0.y() );
			}
			
			break;
		}
		default:
			throw airspace_error("Unknown area descriptor!");
		}
	}
	return pts_ref;
}

/*!
 * Calculate the bearing from _*canvas*_ point p1 to point p2 (degrees)
 *
 * The points are already converted to canvas space which has an upside down
 * geometry ie +ve y is down. Bearings are still however measured in clockwize
 * rotation from the y-axis;
 * 
 */
double Airspace::bearing(QPoint p1, QPoint p2)const 
{
	double dy = p2.y() - p1.y();
	double dx = p2.x() - p1.x();

	double a = 
		PI - 
		(
			dx != 0 
			? atan(dy / dx) 
			: (dy < 0 ? - PI/2 : PI/2) 
		);

	if ( dx >= 0 ) a += PI * ( dy < 0 ? -1 : 1);
	// slightly confusing because canvas space (like screen) is upside down

	return a * 180 / PI;
}


////////////////////////////////////////////////////////////////////////////////
//
// atc::Airspace::AdvancePosition
//
//------------------------------------------------------------------------------
// [public] interface
//

/*!
 * 
 */
void Airspace::AdvancePosition::operator ()( ACAgentMapPair pair ) const { 
	pair.second->advance_position( _time );
}


////////////////////////////////////////////////////////////////////////////////
//
// atc::Airspace::DetectConflicts
//
//------------------------------------------------------------------------------
// [public] interface
//

/*!
 *
 */
Airspace::DetectConflicts::DetectConflicts( 
	    ACAgentMap *map, double hdoms, double vdoms
)
	  : _map( map ), _hdoms( hdoms), _vdoms( vdoms )
{}

/*!
 *
 */
void Airspace::DetectConflicts::operator ()( ACAgentMapPair pair ) const {
	AircraftAgent *ac = pair.second;
	ac->clear_conflict();
	if ( 
			_map->end() != std::find_if( 
					_map->begin(), _map->end()
				  , IsConflicted( ac, _hdoms, _vdoms ) 
			)
	) {
		ac->set_conflict();
	}
}


////////////////////////////////////////////////////////////////////////////////
//
// atc::Airspace::IsConflicted
//
//------------------------------------------------------------------------------
//  atc::Canvas::IsConflicted
//

/*!
 *
 */
Airspace::IsConflicted::IsConflicted( 
		AircraftAgent *ac, double hdoms, double vdoms 
)
	  : _ac( ac ), _hdoms( hdoms ), _vdoms( vdoms ) 
{}

/*!
 *
 */
bool Airspace::IsConflicted::operator ()( ACAgentMapPair pair ) const {
	AircraftAgent *ac = pair.second;
	if ( ac == _ac ) return false;
	return ac->collides_with( _ac, _hdoms, _vdoms );
}


////////////////////////////////////////////////////////////////////////////////
//
// atc::Airspace::InitialControlState
//
//------------------------------------------------------------------------------
//  [public]
//

/*!
 *
 */
void Airspace::InitialControlState::operator ()( ACAgentMapPair pair ) const {
	AircraftAgent *ac = pair.second;

	bool p0  = _airspace->point_in_sector( ac->position() );
	bool p2  = _airspace->point_in_sector( ac->position( TWO_MINUTES ) );
	bool p15 = _airspace->point_in_sector( ac->position( FIFTEEN_MINUTES ) );

	ControlState cs = 
			( !p0 && !p2 && !p15 ) ? CS_NONE
		  : ( !p0 && !p2         ) ? CS_ANNOUNCED
		  : ( !p0 && (ac->AcceptanceType() == ACCEPTANCE_MANUAL)) ? CS_PROPOSED
		  : (  p0 && !p2         ) ? CS_OVEROUT
		  :                          CS_ACCEPTED
	;
	ac->set_control( cs );
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//
// atc::Airspace::UpdateContolState
//
//--------------------------------------------------------------------------------------------------
//  [public]
//

/*!
 *
 */
void Airspace::UpdateControlState::operator ()( ACAgentMapPair pair ) const {
	AircraftAgent *ac = pair.second;
	if ( ac->is_active() )
		ac->set_control( updated_control( ac ) );
}

//--------------------------------------------------------------------------------------------------
//  [private]
//

/*!
 *
 */
ControlState Airspace::UpdateControlState::updated_control( AircraftAgent *ac ) const {
	bool p0  = _airspace->point_in_sector( ac->position() );
	bool p2  = _airspace->point_in_sector( ac->position( TWO_MINUTES ) );
	bool p15 = _airspace->point_in_sector( ac->position( FIFTEEN_MINUTES ) );

	ControlState cs = ac->control();

	switch ( cs ) 
	{
	case CS_NONE:      
		if (p15) 
		{
			return CS_ANNOUNCED;
		}

	case CS_ANNOUNCED: 
		if (p2) 
		{
			if (ac->AcceptanceType() == ACCEPTANCE_MANUAL)
			{
				return CS_PROPOSED;
			}
			else
			{
				return CS_ACCEPTED;
			}
		}

	case CS_ACCEPTED:  
		if ( p0 && !p2 ) 
		{
			return CS_OVEROUT;
		}

	default:                            
		return cs;
	}
}


////////////////////////////////////////////////////////////////////////////////////////////////////
